#!/usr/bin/env python3
import rospy
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
import math

class GoOneMeter():
    def __init__(self, distance):
        self.distance = distance
        self.sub = rospy.Subscriber("odom", Odometry, self.controlCallback, queue_size=1)   # 监听
        self.pub = rospy.Publisher("cmd_vel", Twist, queue_size=1)  # 发布
        self.x = None   # 实际x
        self.y = None   # 实际y
        self.originX = None # 开始x
        self.originY = None # 开始y
        self.twist = Twist() # 发布对象

    def controlCallback(self,data):
        # 获取初始坐标
        if self.originX == None:
            self.originX = data.pose.pose.position.x
            self.originY = data.pose.pose.position.x
        
        # 获取当前坐标
        self.x = data.pose.pose.position.x
        self.y = data.pose.pose.position.y

        # 计算移动距离判断退出条件
        dist = math.hypot(self.originX - self.x,self.originY-self.y)
        if dist >= self.distance:
            rospy.signal_shutdown("over")

        # 发布信息
        self.twist.linear.x = 0.2
        self.pub.publish(self.twist)
        print("distance:",dist)

if __name__ == "__main__":
    rospy.init_node("go_one_meter_line",anonymous=False)    # 初始化节点
    goOneMeter = GoOneMeter(1)  # 实例化类，传入移动距离
    rospy.spin()                # 启动监听